#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Created on Thu Sep 24 20:32:41 2020

@author: bing
"""

import os
import glob
import re

# parse data from rosbag
if os.path.exists('topics.yaml')==False:
    os.system('./ros_readbagfile.py *.bag | tee topics.yaml')

# obtain all jpg files' name to a list
jpgfilenames = glob.glob(r"./Jpgs_path/*.JPG")

# open jpg position log
file_jpg_position_log = open('jpg_position_log.txt', 'wt')

# find the modify time of all the jpgs
for jpg_file_name in jpgfilenames:
    print(jpg_file_name)
    file_jpg_position_log.write(jpg_file_name + ' ')
    # open ros topic files
    file_rostopics_txt = open('topics.yaml','rt',encoding='utf8')
    jpg_file_modify_time = os.path.getmtime(jpg_file_name)
    file_line_num = 0
    topic_line_num = 0
    right_time_line_num = 0
    flag_right_time_ready = 0
    for lines in file_rostopics_txt.readlines():
        file_line_num = file_line_num + 1
        if lines.find('topic:           /uwb_msg')!=-1:
#            print(file_line_num)
            topic_line_num = file_line_num
            continue
        if file_line_num == (topic_line_num+2) and file_line_num>100 and flag_right_time_ready==0:
            topic_time = re.sub("[^0-9.]","",lines)
            if (int(float(topic_time)) == int(jpg_file_modify_time)):
                right_time_line_num = file_line_num
                flag_right_time_ready = 1
                continue
        if file_line_num == (right_time_line_num + 8) and file_line_num>100 and flag_right_time_ready==1:
            tmp = re.sub("[^0-9.]"," ",lines)
            tmp = list(filter(None,tmp.split(" ")))
            print(tmp[1])
            file_jpg_position_log.write(tmp[0] + ' ' + tmp[1] + '\n')
    file_rostopics_txt.close()

file_jpg_position_log.close()